Estimation of a position and orientation of a frame used in controlling movement of a tool

ABSTRACT

A robotic system includes a camera having an image frame whose position and orientation relative to a fixed frame is determinable through one or more image frame transforms, a tool disposed within a field of view of the camera and having a tool frame whose position and orientation relative to the fixed frame is determinable through one or more tool frame transforms, and at least one processor programmed to identify pose indicating points of the tool from one or more camera captured images, determine an estimated transform for an unknown one of the image and tool frame transforms using the identified pose indicating points and known ones of the image and tool frame transforms, update a master-to-tool transform using the estimated and known ones of the image and tool frame transforms, and command movement of the tool in response to movement of a master using the updated master-to-tool transform.

FIELD OF THE INVENTION

The present invention generally relates to robotic systems and inparticular, to estimation of a position and orientation of a frame usedin controlling operator commanded movement of a tool.

BACKGROUND OF THE INVENTION

Medical robotic systems such as those used in performing minimallyinvasive surgical procedures offer many benefits over traditional opensurgery techniques, including less pain, shorter hospital stays, quickerreturn to normal activities, minimal scarring, reduced recovery time,and less injury to tissue. Consequently, demand for minimally invasivesurgery using such medical robotic systems is strong and growing.

Examples of medical robotic systems include the DaVinci® SurgicalSystem, the daVinci S® Surgical System, and the DaVinci® Si HD™ SurgicalSystem from Intuitive Surgical, Inc., of Sunnyvale, Calif. Each of thesesystems includes a surgeon's workstation, a patient-side cart, a highperformance three-dimensional (“3-D”) vision system, and IntuitiveSurgical's proprietary EndoWrist® articulating instruments, which aremodeled after the human wrist so that when added to the motions of theslave manipulator holding the surgical instrument, they allow at least afull six degrees of freedom of motion, which is comparable to or evengreater than the natural motions of open surgery.

The DaVinci® surgeon's workstation has a high-resolution stereoscopicvideo display. The system offers higher fidelity than polarization,shutter eyeglass, or other 3-D display techniques. Each eye views aseparate display that presents the left or right eye perspective throughan objective lens and a series of mirrors. The surgeon sits comfortablyand looks into this display throughout surgery, making it an ideal placefor the surgeon to display and manipulate 3-D intraoperative imagery.

The patient-side cart typically includes three or more slavemanipulators for holding and manipulating medical devices such assurgical instruments and image capturing devices for performing andviewing a medical procedure at a surgical site within a patient. Tomanipulate these medical devices, the surgeon's workstation alsoincludes master control devices which may be selectively associated withthe medical devices (and the slave manipulators holding the medicaldevices) to manipulate them.

In such a medical robotic system, as well as other robotic systems ingeneral, the control of a surgical instrument in response to operatormanipulation of a master control device may have a number of definablereference frames and corresponding frame transforms to map points in oneframe to corresponding points in another frame. When the position andorientation of one of the frames is unknown, however, precise control ofthe surgical instrument may be difficult to achieve so that the safetyof a patient being treated at the time by the medical robotic system aswell as the successful completion of a procedure being performed on thepatient may be jeopardized.

OBJECTS AND SUMMARY OF THE INVENTION

Accordingly, one object of aspects of the present invention is a roboticsystem and a method implemented in the robotic system for estimating aposition and orientation of a frame used in controlling operatorcommanded movement of a tool whose image is being captured and displayedto the operator at the time.

This and additional objects are accomplished by the various aspects ofthe present invention, wherein briefly stated, one aspect is a roboticsystem comprising: a master control device having a master frame; animage capturing system having an image frame whose position andorientation relative to a fixed frame is determinable through one ormore image frame transforms; a tool disposed within a field of view ofthe image capturing system and having a tool frame whose position andorientation relative to the fixed frame is determinable through one ormore tool frame transforms; and at least one processor programmed toidentify pose indicating points of the tool from information in one ormore images captured by the image capturing system, determine anestimated transform for an unknown one of the image and tool frametransforms by using the identified pose indicating points and known onesof the image and tool frame transforms, update a master-to-tooltransform by using the estimated and known ones of the image and toolframe transforms, and command movement of the tool in response tomovement of the master control device by using the updatedmaster-to-tool transform.

Another aspect is a method implemented in a robotic system comprising amaster control device having a master frame, an image capturing systemhaving an image frame whose position and orientation relative to a fixedframe is determinable through one or more image frame transforms, a tooldisposed within a field of view of the image capturing system and havinga tool frame whose position and orientation relative to the fixed frameis determinable through one or more tool frame transforms, the methodcomprising: identifying pose indicating points of the tool frominformation in one or more images captured by the image capturingsystem; determining an estimated transform for an unknown one of theimage and tool frame transforms by using the identified pose indicatingpoints and known ones of the image and tool frame transforms; updating amaster-to-tool transform by using the estimated and known ones of theimage and tool frame transforms; and commanding movement of the tool inresponse to movement of the master control device by using the updatedmaster-to-tool transform.

Additional objects, features and advantages of the various aspects ofthe present invention will become apparent from the followingdescription of a preferred embodiment, which description should be takenin conjunction with the accompanying drawings.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 illustrates a top view of an operating room employing a medicalrobotic system utilizing aspects of the present invention.

FIG. 2 illustrates a schematic three-dimensional drawing indicating thepositions of the end effectors relative to a viewing end of an endoscopeand the corresponding positions of master control devices relative tothe eyes of an operator.

FIG. 3 illustrates a schematic three-dimensional drawing indicating theposition and orientation of an end effector relative to an image frameof an image capturing system.

FIG. 4 illustrates a schematic three-dimensional drawing indicating theposition and orientation of a pincher formation of a master controldevice relative to an eye reference system associated with the eyes ofan operator of the medical robotic system.

FIG. 5 illustrates a schematic side view of part of a surgical stationindicating locations of frames used by a control system to determine theposition and orientation of an end effector of a tool relative to animage frame associated with an image capturing system.

FIG. 6 illustrates a schematic side view of part of a surgeon'sworkstation 200 indicating locations of frames used by a control systemto determine the position and orientation of a pincher formation of amaster control device relative to an eye frame associated with the eyesof the surgeon.

FIGS. 7-9 schematically illustrate corresponding mapping locations onthe surgeon's hand, on the master control device, and on the endeffector of a tool and methods for their selection.

FIG. 10 illustrates a block diagram of a high level control architecturemodel of a master/slave robotic system.

FIG. 11 illustrates a schematic side view of left and right toolsinserted along with an endoscope through curved cannulas so that thetools and endoscope pivot about a common pivot point located at a commonentry port.

FIG. 12 illustrates a schematic side view of an endoscope tethered at awork site to view a tool.

FIG. 13 illustrates a schematic side view of movable structures forrespectively positioning a robotically controlled tool and imagecapturing system at a work site.

FIG. 14 illustrates a schematic diagram of image frame transforms usedfor transforming points in an image frame to a fixed frame and toolframe transforms used for transforming points in a tool frame to thefixed frame.

FIG. 15 illustrates a flow diagram of a method for estimating an unknownframe transform for commanding movement of a tool in response tomovement of a master control device.

DETAILED DESCRIPTION

Although the following example describes application of the presentinvention to a medical robotic system, it is to be appreciated that thepresent invention is not to be so limited. In particular, the presentinvention is applicable to robotic systems in general and should beaccorded its full scope according to the attached claims.

FIG. 1 illustrates, as an example, a top view of an operating roomemploying a medical robotic system 100. The medical robotic system inthis case is a minimally invasive robotic surgical system including aworkstation 200 utilized by a surgeon (“S”) while performing a medicalprocedure, such as a diagnostic or surgical procedure, with assistancefrom one or more assistants (“A”), on a patient (“P”) who is lying faceup on an operating table 115.

The workstation 200 includes a 3-D display 202 for displaying a 3-Dimage of a surgical or work site to the surgeon, left and right mastercontrol devices 700, 700, a foot pedal 105, and a processor 102. Thecontrol devices 700, 700 may include any one or more of a variety ofinput devices such as joysticks, gloves, trigger-guns, hand-operatedcontrollers, or the like. In the present example, they are implementedas pinchers that may be opened and closed and moved in multiple degreesof freedom to correspond to desired movement of their respectivelyassociated surgical tools. The processor 102 may be a dedicated computerintegrated into the workstation 200 or positioned next or near to it, orthe processor 102 may be broken up into a number of processing orcontroller components that are distributed in a distributed processingfashion throughout the system 100.

The surgeon performs a medical procedure by manipulating the mastercontrol devices 700, 700 (also referred to as “master manipulators”) sothat the processor 102 causes slave manipulators of their respectivelyassociated robotic arm assemblies 128, 129 to manipulate theirrespective removably coupled tools 14, 14 (also referred to as “surgicalinstruments”) accordingly, while the surgeon views the surgical site in3-D on the workstation 200 display 202 as it is captured by an imagecapturing device, which in the present example, is a stereoscopicendoscope 304 having a pair of cameras which capture correspondingimages for stereo vision.

Each of the tools 14, 14, as well as the endoscope 304, isconventionally inserted in this example through a tool guide into thepatient so as to extend down to the surgical site through acorresponding minimally invasive incision such as incision 166. Thenumber of surgical tools used at one time and consequently, the numberof robotic arms being used in the system 100 will generally depend onthe medical procedure being performed and the space constraints withinthe operating room, among other factors. If it is necessary to change atool being used during a procedure, the assistant may remove the tool nolonger being used from its robotic arm assembly, and replace it withanother tool 14 from a tray 116 in the operating room.

So that the tools 14, 14 may be manipulated at the surgical site, theyeach have a wrist mechanism including joints (or other drivablemechanical elements such as gears, spools, etc.) for controlling theorientation of the wrist mechanism, and an additional joint (or otherdrivable mechanical element) controlling the eventual grip or other endeffector joint of the tool. For additional details on such a tool wristand end effector mechanism (and the mechanical elements and otherlinkages driving them), see, e.g., U.S. Pat. No. 7,780,651 (filed Nov.27, 2007; entitled “Articulated Surgical Instrument for PerformingMinimally Invasive Surgery with Enhanced Dexterity and Sensitivity”),U.S. Pat. No. 6,936,902 (filed Apr. 16, 2002; entitled “Surgical Toolsfor Use in Minimally Invasive Telesurgical Applications”), and U.S. Pat.No. 7,736,356 (filed Nov. 1, 2004; entitled “Surgical Tool HavingPositively Positionable Tendon-Actuated Multi-Disk Wrist Joint”), whichare incorporated herein by reference.

Each of the robotic arm assemblies 122, 128, 129 includes a slave arm(also referred to as a “slave manipulator”) and a setup arm. The slavemanipulators are robotically moved using motor controlled joints (alsoreferred to herein as “active joints”) in order to manipulate and/ormove their respectively held medical devices. The setup arms may bemanually manipulated by releasing normally braked joints (also referredto herein as “setup joints”) to horizontally and vertically position therobotic arm assemblies 122, 128, 129 so that their respective medicaldevices may be inserted into their respective tool guides.Alternatively, the setup arms may be actively controlled using motorcontrolled joints.

The display 202 is positioned near the surgeon's hands so that it willdisplay a projected image that is oriented so that the surgeon feelsthat he or she is actually looking directly down onto the operatingsite. To that end, images of the tools 14, 14 appear to be locatedsubstantially where the surgeon's hands are located to give a sense oftelepresence to the surgeon (e.g., the perception that the mastercontrol devices are integral with the surgical tools).

The processor 102 performs various functions in the system 100. Oneimportant function that it performs is to translate and transfer themechanical motion of control devices 700, 700 to their respective slavemanipulators of robotic arm assemblies 128, 129 through control signalsover bus 110 so that the surgeon can effectively manipulate theirrespective tools 14, 14. Another important function is to implementvarious control system processes and methods as described herein.

Although described as a processor, it is to be appreciated that theprocessor 102 may be implemented in practice by any combination ofhardware, software, and firmware. Also, the processor functions asdescribed herein may be performed by one unit or divided up amongdifferent components, each of which may be implemented in turn by anycombination of hardware, software and firmware.

For additional details on the construction and operation of medicalrobotic systems such as described herein, see, e.g., U.S. Pat. No.6,493,608 (filed Apr. 7, 1999; entitled “Aspects of a Control System ofa Minimally Invasive Surgical Apparatus”) and U.S. Pat. No. 6,424,885(filed Aug. 13, 1999; entitled “Camera Referenced Control in a MinimallyInvasive Surgical Apparatus”), which are incorporated herein byreference.

In use, and as schematically indicated in FIG. 2 of the drawings, thesurgeon views the surgical site through the display 202. The endeffector 58 carried on each tool 14 is caused to perform positional andorientational movements in response to movement and action inputs on itsassociated master control device. The master control devices areindicated schematically at 700, 700. It will be appreciated that duringa surgical procedure images of the end effectors 58 are captured by theendoscope 304 together with the surgical site and are displayed on thedisplay 202 so that the surgeon sees the responsive movements andactions of the end effectors 58 as he or she controls such movements andactions by means of the master control devices 700, 700. The controlsystem (as described in reference to FIG. 10 below) is arranged to causeend effector orientational and positional movement as viewed in theimage at the display 202 to be mapped onto orientational and positionalmovement of a pincher formation of the master control device as will bedescribed in greater detail below.

The operation of the control system of the minimally invasive roboticsurgical system will now be described in greater detail. In thedescription which follows, the control system will be described withreference to a single master control device 700 and its associatedrobotic arm and surgical instrument 14. The master control device 700will be referred to simply as “master” and its associated robotic armand surgical instrument 14 will be referred to simply as “slave.”

The method whereby control between master movement and correspondingslave movement is achieved by the control system of the minimallyinvasive surgical system 100 will now be described with reference toFIGS. 2-4 of the drawings in overview fashion. The method will then bedescribed in greater detail with reference to FIGS. 5-15 of thedrawings.

Control between master and slave movement is achieved by comparingmaster position and orientation in an eye Cartesian coordinate referencesystem (also referred to herein as the “eye frame”) with slave positionand orientation in a camera Cartesian coordinate reference system (alsoreferred to herein as the “camera frame” or more generally, “imageframe”). For ease of understanding and economy of words, the term“Cartesian coordinate reference system” will simply be referred to as“frame” in the rest of this specification including the attached claims.Accordingly, when the master is stationary, the slave position andorientation within the camera frame is compared with the master positionand orientation in the eye frame, and if the position and/or orientationof the slave in the camera frame does not correspond with the positionand/or orientation of the master in the eye frame, the slave is causedto move to a position and/or orientation in the camera frame at whichits position and/or orientation in the camera frame does correspond withthe position and/or orientation of the master in the eye frame. In FIG.3, the camera frame is generally indicated by reference numeral 610 andthe eye frame is generally indicated by reference numeral 612 in FIG. 4.Similarly, in a master alignment mode, the slave is stationary and themaster is caused to move to a position and/or orientation in the eyeframe that corresponds to the slave position and orientation in thecamera frame.

When the master is moved into a new position and/or orientation in theeye frame 612, the new master position and/or orientation does notcorrespond with the previously corresponding slave position and/ororientation in the camera frame 610. The control system then causes theslave to move into a new position and/or orientation in the camera frame610 at which new position and/or orientation, its position andorientation in the camera frame 610 does correspond with the newposition and/or orientation of the master in the eye frame 612.

It will be appreciated that the control system includes at least one,and typically a plurality, of processors which compute new correspondingpositions and orientations of the slave in response to master movementinput commands on a continual basis determined by the processing cyclerate of the control system. A typical processing cycle rate of thecontrol system under discussion is about 1300 Hz. Thus, when the masteris moved from one position to a next position, the correspondingmovement desired by the slave to respond is computed at about 1300 Hz.Naturally, the control system can have any appropriate processing cyclerate depending on the processor or processors used in the controlsystem. In one implementation, all real-time servocycle processing isconducted on a DSP (Digital Signal Processor) chip. DSPs are used inthis case because of their constant calculation predictability andreproducibility. A DSP from Analog Devices, Inc. of Massachusetts is anacceptable example of such a processor for performing the functionsdescribed herein.

The camera frame 610 is positioned such that its origin 614 ispositioned at the viewing end 306 of the endoscope 304. Conveniently,the z axis of the camera frame 610 extends axially along a viewing axis616 of the endoscope 304. Although in FIG. 3, the viewing axis 616 isshown in coaxial alignment with a shaft axis of the endoscope 304, it isto be appreciated that the viewing axis 616 can be angled relativethereto. Thus, the endoscope can be in the form of an angled scope.Naturally, the x and y axes are positioned in a plane perpendicular tothe z axis. The endoscope is typically angularly displaceable about itsshaft axis. The x, y and z axes are fixed relative to the viewing axisof the endoscope 304 so as to displace angularly about the shaft axis insympathy with angular displacement of the endoscope 304 about its shaftaxis.

To enable the control system to determine slave position andorientation, a frame is defined on or attached to the end effector 58.This frame is referred to as an end effector frame or slave tip frame orsimply tool frame, in the rest of this specification, and is generallyindicated by reference numeral 618. The end effector frame 618 has itsorigin at the pivotal connection 60. Conveniently, one of the axes e.g.the z axis, of the frame 618 is defined to extend along an axis ofsymmetry, or the like, of the end effector 58. Naturally, the x and yaxes then extend perpendicularly to the z axis. It will be appreciatedthat the orientation of the slave is then defined by the orientation ofthe frame 618 having its origin at the pivotal connection 60, relativeto the camera frame 610. Similarly, the position of the slave is thendefined by the position of the origin of the frame at 60 relative to thecamera frame 610.

Referring now to FIG. 4 of the drawings, the eye frame 612 is chosensuch that its origin corresponds with a position 201 where the surgeon'seyes are normally located when he or she is viewing the surgical site atthe display 202. The z axis extends along a line of sight of thesurgeon, indicated by axis 620, when viewing the surgical site throughthe display 202. Naturally, the x and y axes extend perpendicularly fromthe z axis at the origin 201. Conveniently, the y axis is chosen toextend generally vertically relative to the display 202 and the x axisis chosen to extend generally horizontally relative to the display 202.

To enable the control system to determine master position andorientation within the display frame 612, a point on the master ischosen which defines an origin of a master or master tip frame,indicated by reference numeral 622. This point is chosen at a point ofintersection indicated by reference numeral 3A where the axes ofrotation of the master intersect. Conveniently, the z axis of the masterframe 622 on the master extends along an axis of symmetry of the pincherformation 706 which extends coaxially along the rotational axis I. The xand y axes then extend perpendicularly from the axis of symmetry 1 atthe origin 3A. Accordingly, orientation of the master within the eyeframe 612 is defined by the orientation of the master frame 622 relativeto the eye frame 612. The position of the master in the eye frame 612 isdefined by the position of the origin 3A relative to the eye frame 612.

How the position and orientation of the slave within the camera frame610 is determined by the control system will now be described withreference to FIG. 5 of the drawings. FIG. 5 shows a schematic diagram ofone of the robotic arm and surgical instrument 14 assemblies mounted onthe patient-side cart 300. However, before commencing with a descriptionof FIG. 5, it is appropriate to describe certain previously mentionedaspects of the patient-side cart 300 which impact on the determinationof the orientation and position of the slave relative to the cameraframe 610.

In use, when it is desired to perform a surgical procedure by means ofthe minimally invasive surgical system, the patient-side cart 300 ismoved into close proximity to a patient requiring the surgicalprocedure. The patient is normally supported on a surface such as anoperating table, or the like. To make allowance for support surfaces ofvarying height, and to make allowance for different positions of thepatient-side cart 300 relative to the surgical site at which thesurgical procedure is to be performed, the patient-side cart 300 isprovided with the ability to have varying initial setup configurations.Accordingly, the tool slave arms 10, 12, and the endoscope slave arm 302are mounted on the carriage 97 which is heightwise adjustable, asindicated by arrows K, relative to the base 99 of the cart 300.Furthermore, the tool slave arms 10, 12 and the endoscope slave arm 302are mounted on the carriage 97 by means of their respective setup arms95. Thus, the lateral position and orientation of the slave arms 10, 12,302 can be selected by moving the setup arms 95. Thus, at thecommencement of the surgical procedure, the cart 300 is moved into theposition in close proximity to the patient, an appropriate height of thecarriage 97 is selected by moving it to an appropriate height relativeto the base 99 and the surgical instruments 14 are moved relative to thecarriage 97 so as to introduce the shafts of the instruments 14 and theendoscope 304 through the ports of entry and into positions in which theend effectors 58 and the viewing end 306 of the endoscope 304 areappropriately positioned at the surgical site and the fulcrums (i.e.,pivot points) are coincident with the ports of entry. Once the heightand positions are selected, the carriage 97 is locked at its appropriateheight and the setup arms 95 are locked in their positions andorientations. Normally, throughout the surgical procedure, the carriage97 is maintained at the selected height and similarly the setup arms 95are maintained in their selected positions. However, if desired, eitherthe endoscope or one or both of the instruments can be introducedthrough other ports of entry during the surgical procedure.

The determination by the control system of the position and orientationof the slave within the camera frame 610 will now be described. It willbe appreciated that this is achieved by means of one or more processorshaving a specific processing cycle rate. Thus, where appropriate,whenever position and orientation are referred to in this specification,it should be borne in mind that a corresponding velocity is also readilydetermined. The control system determines the position and orientationof the slave within the camera frame 610 by determining the position andorientation of the slave relative to a cart frame 624 and by determiningthe orientation and position of the endoscope 304 with reference to thesame cart frame 624. The cart frame 624 has an origin indicated byreference numeral 626 in FIG. 5.

To determine the position and orientation of the slave relative to thecart frame 624, the position of a fulcrum frame 630 having its origin atthe fulcrum 49 is determined within the cart frame 624 as indicated bythe arrow 628 in dashed lines. It will be appreciated that the positionof the fulcrum 49 normally remains at the same location, coincident witha port of entry into the surgical site, throughout the surgicalprocedure. The position of the end effector frame 618 on the slave,having its origin at the pivotal connection 60 (as shown in FIG. 3), isthen determined relative to the fulcrum frame 630 and the orientation ofthe end effector frame 618 on the slave is also determined relative tothe fulcrum frame 630. The position and orientation of the end effectorframe 618 relative to the cart frame 624 is then determined by means ofroutine calculation using trigonometric relationships.

It will be appreciated that the slave arm 302 of the endoscope 304 isconstrained to move in similar fashion to the tool slave arm 10. Thus,the endoscope 304 when positioned with its viewing end 306 directed atthe surgical site, also defines a fulcrum coincident with its associatedport of entry into the surgical site. The endoscope slave arm 302 can bedriven to cause the endoscope 304 to move into a different positionduring a surgical procedure, to enable the surgeon to view the surgicalsite from a different position in the course of performing the surgicalprocedure. It will be appreciated that movement of the viewing end 306of the endoscope 304 is performed by varying the orientation of theendoscope 304 relative to its pivot center or fulcrum. Operator controlof such movement may be performed by switching associations of one orboth master control devices 700, 700 from the tools 14, 14 to theendoscope 304. Once the endoscope 304 is moved to the desired positionand orientation, the slave arm 304 may be locked in place andassociations of the master control devices 700, 700 may be switched backto their respective tools. Alternatively, the foot pedal 105 or otherconventional means may be used to position the endoscope 304. Theposition and orientation of the camera frame 610 within the cart frame624 is determined in similar fashion to the position and orientation ofthe slave within the cart frame 624. When the position and orientationof the camera frame 610 relative to the cart frame 624, and the positionand orientation of the slave relative to the cart frame 624 have beendetermined in this manner, the position and the orientation of the slaverelative to the camera frame 610 is readily determinable through routinecalculation using trigonometric relationships.

How the position and orientation of the master within the display frame612 is determined by the control system will now be described withreference to FIG. 6 of the drawings. FIG. 6 shows a schematic diagram ofone of the master control devices 700 at the operator workstation 200.

The workstation 200 optionally also includes setup arms, as indicated at632, to enable the general location of the masters 700, 700 to be variedto suit the surgeon. Thus, the general position of the masters 700, 700can be selectively varied to bring the masters 700, 700 into a generalposition at which they are comfortably positioned for the surgeon. Whenthe masters 700, 700 are thus comfortably positioned, the setup arms 632are locked in position and are normally maintained in that positionthroughout the surgical procedure.

To determine the position and orientation of the master 700, asindicated in FIG. 6, within the eye frame 612, the position andorientation of the eye frame 612 relative to a surgeon's station frame634, and the position and orientation of the master 700 relative to thesurgeon's frame 634 is determined. The surgeon's station frame 634 hasits origin at a location which is normally stationary during thesurgical procedure, and is indicated at 636.

To determine the position and orientation of the master 700 relative tothe station frame 634, a position of a master setup frame 640 at an endof the setup arms 632 on which the master 700 is mounted, relative tothe station frame 636, is determined, as indicated by the arrow 638 indashed lines. The position and orientation of the master frame 622 onthe master 700 having its origin at 3A is then determined relative tothe master setup frame 640. In this manner, the position and orientationof the master frame 622 relative to the station frame 634 can bedetermined by means of routine calculation using trigonometricrelationships. The position and orientation of the eye frame 612relative to the station frame 634 is determined in similar fashion. Itwill be appreciated that the position of the display 202 relative to therest of the surgeon's workstation 200 can selectively be varied to suitthe surgeon. The position and orientation of the master frame 622relative to the eye frame 612 can then be determined from the positionand orientation of the master frame 622 and the eye frame 612 relativeto the surgeon station frame 634 by means of routine calculation usingtrigonometric relationships.

In the manner described above, the control system of the minimallyinvasive surgical apparatus determines the position and orientation ofthe end effector 58 by means of the end effector frame 618 in the cameraframe 610, and, likewise, determines the position and orientation of thepinchers of the master by means of the master frame 622 relative to theeye frame 612.

As mentioned, the surgeon grips the master by locating his or her thumband index finger over the pincher formation 706 (see FIG. 4). When thesurgeon's thumb and index finger are located on the pincher formation,the point of intersection 3A is positioned inwardly of the thumb andindex finger tips. The master frame having its origin at 3A iseffectively mapped onto the end effector frame 618, having its origin atthe pivotal connection 60 of the end effector 58 as viewed by thesurgeon in the display 202. Thus, when performing the surgicalprocedure, and the surgeon manipulates the position and orientation ofthe pincher formation 706 to cause the position and orientation of theend effector 58 to follow, it appears to the surgeon that his or herthumb and index finger are mapped onto the fingers of the end effector58 and that the pivotal connection 60 of the end effector 58 correspondswith a virtual pivot point of the surgeon's thumb and index fingerinwardly from the tips of the thumb and index finger. It will beappreciated that depending upon the actual configuration of the pincherformation, in particular the point of intersection of the rotation axesrelative to the position of the pincher formation 706, the frame 622 onthe master 700 can be offset from the intersection 3A so as to approacha point relative to the surgeon's hand at which point the pivotalconnection 60 approximately corresponds.

Accordingly, as the surgical procedure is being performed the positionand orientation of the fingers of the end effector track position andorientation changes of the surgeon's thumb and index finger in anaturally intuitive or superimposed fashion. Furthermore, actuation ofthe end effector 58, namely causing the end effector fingers selectivelyto open and close, corresponds intuitively to the opening and closing ofthe surgeon's thumb and index finger. Thus, actuation of the endeffector 58 as viewed in the display 202 is performed by the surgeon ina naturally intuitive manner, since the pivot point 60 of the endeffector 58 is appropriately mapped onto a virtual pivot point betweenthe surgeon's thumb and index finger.

It will be appreciated that the end effector frame 618 can, whereappropriate, be offset relative to the pivotal connection 60. Thus, forexample, if the end effector (as shown in the display) has fingers of arelatively long length, the origin of the end effector frame can beoffset in a direction toward the end effector finger tips. It will alsobe appreciated that by using positional and/or orientational offsetsbetween the master frame 622 and the intersection 3A, as well as betweenthe end effector frame 618 and the pivotal connection 60, the mapping ofthe pincher formation 706 onto the end effector 58 may be shifted, forexample to map the tips of the pincher formation onto the tips of theend effector. These alternative mappings are illustrated in FIG. 7.

Generally, a first pincher element 706A will be substantially connectedto a first end effector element 58.1, while a second pincher element706B is substantially connected to a second end effector element 58.2.Optionally, point 3A (which is ideally near the center of rotation ofthe gimbal structure of master 700, 706A, and 706B), adjacent thepivotal connection between the pincher elements, may be substantiallyconnected with pivotal connection 60 on the slave. This also effectivelyprovides a substantial connection between the pivot point on thesurgeon's hand H and pivotal connection 60, as the surgeon will oftengrip the master with the hand's pivot point (at the base of thesurgeon's finger and thumb) disposed along the pivot point of thepincher. Alternatively, midpoint MP1 disposed between the tips of thepincher elements may be substantially connected to midpoint MP2 disposedbetween the tips of the end effector elements. Each of the higher levelsof connection described herein may optionally be provided by thismapping.

FIGS. 8 and 9 more clearly illustrate corresponding mapping pointsbetween the handle of the master controller and end effector of theslave, while FIG. 9 schematically illustrates method steps for selectingthese corresponding mapping points. In general, interchangeable endeffectors having different end effector element lengths may beaccommodated by varying the mapping point of the handle or the endeffector. Such variation in mapping points may also be used when themagnification of the image shown at the display changes significantly.For example, substantial connection of pivotal connection 60 of the endeffector and intersection 3A of the handle may be appropriate when theend effector is shown at a first magnification, but may be inappropriatewhen magnification of the end effector is increased significantly, orwhen an alternative end effector having longer end effector elements isattached to the slave. In either circumstance, it may be appropriate toalter the master/slave interaction to substantially connect midpoint MP2of the master to midpoint MP1′ of the end effector, as illustrated inFIG. 8.

As a preliminary matter, it is beneficial in robotic surgery systems toprovide a master controller having a gimbal point GP adjacent the handleto be gripped by the surgeon. This avoids large master inertia when thesurgeon rapidly rotates the handle, as often occurs during surgicalprocedures. By having a master which has multiple degrees of freedomintersecting at the gimbal point GP (ideally having three orientationaldegrees of freedom intersecting at the gimbal point), and by having thegimbal point coincident with the handle, inertia of rapid rotationalmovements at the master can be quite low.

As described above, it is often beneficial to coordinate movements ofthe slave so that an image of pivotal connection 60 of the slave appearssubstantially connected to pincher formation pivot point 3A between thepincher or grip elements 706A, 706B. However, when end effector elements58.1, 58.2 extend a considerable distance beyond pivotal connection 60(as shown in the display adjacent the master controller), the surgeonmay feel that manipulation of these long end effector elements from thedistant pivotal connection becomes awkward. Similarly, when manipulatinga single end effector element such as a scalpel which is much longer (asdisplayed at the master control station) than the master handle, thesurgeon may be given the impression of cutting with a long-handledsword, rather than an easily controlled scalpel. As described above, onealternative to overcome an awkward disparity in grip/end effectorlengths is to map the surgical workspace and master controller workspacetogether so that the midpoints MP1, MP2 between the end effector jawends and the handle grip member ends are substantially connected. Bymapping the surgical and master workspace so that these midpoints aresubstantially connected, the surgeon can coordinate movement using theend effector despite significant differences in length between the endeffector elements and the grip elements.

The mapping point need not be limited to any particular point. In theexemplary embodiment, a middle axis of the grip members MAG is generallydefined midway between pincher elements 706A, 706B, while a similarmiddle axis of the end effector MAE is defined midway between the endeffector elements 58.1, 58.2. The mapping point (or point of substantialconnection) of the master will be disposed along gripping middle axisMAG, ideally in a range from intersection 3A to midpoint MP2. Similarly,the mapping or substantial connection point of the end effector will bedisposed along middle axis MAE, ideally in a range from pivotalconnection 60 to midpoint MP1.

FIG. 9 schematically illustrates a method for determining the locationof the substantially connected mapping points along the handle and endeffector. First, the location of the surgeon's hand along the masterhandle is reviewed to determine the position of the surgeon's fingersrelative to the gimbal point GP. In one embodiment, the offset distancebetween a location of the surgeon's fingertips and the gimbal point GPdefines an offset distance. This offset distance is scaled using ascaling factor, typically using a ratio between a length of the gripmembers and the length of the end effector elements, a magnification ofthe display, or the like. For example, using numbers typical of theexemplary robotic surgery system, the offset distance is scaled bymultiplying it by one-third, as the grip members typically have a lengthof about three times the end effector element lengths. This scalingfactor may change with tool changes (when end effectors having longer orshorter end effector elements are used), or the like. The location ofthe mapping points on the slave can then be calculated, for example, ata position offset from midpoint MP1 toward pivotal connection 60 alongthe end effector middle axis MAE by the scaled offset distance. Thismapping point of the end effector may then be substantially connected togimbal point GP of the master.

It will be appreciated that the cart frame 624 can be chosen at anyconvenient location in which its origin corresponds with a location onthe cart 300 which does not vary relative to its base 99. The surgeon'sstation frame 634 can likewise be chosen at any convenient location suchthat its origin is located at a position which does not vary relative toa base 642 thereof. Furthermore, to determine the position andorientation of the camera frame 610 relative to the cart frame 624, usecan be made of a plurality of different intermediate frame paths. Todetermine the position and orientation of the end effector frame 618relative to the cart frame 624 use can also be made of a plurality ofdifferent intermediate frame paths.

However, it has been found that if the intermediate frame paths areappropriately selected, the control system is then arranged to bereadily adaptable to accommodate modular replacement of modular partshaving characteristics different than the characteristics of the modularparts being replaced. It will be appreciated that selecting intermediateframes also eases the computational process involved in determiningmaster and slave position and orientation.

Referring again to FIG. 5, the cart frame is chosen at 624, as alreadymentioned. It will be appreciated that determining the position of thefulcrum frame 630 relative to the cart frame 624 is achieved throughappropriately positioned sensors, such as potentiometers, encoders, orthe like. Conveniently, the fulcrum frame position 630 relative to thecart frame 624 is determined through two intermediate frames. One of theframes is a carriage guide frame 644 which has its origin at aconvenient location on a guide along which the carriage 97 is guided.The other frame, an arm platform frame indicated at 646 is positioned atan end of the setup joint arm 95 on which the slave arm 10 is mounted.Thus, when slave position and orientation is determined relative to thecart frame 624, the carriage guide frame 644 position relative to thecart frame 624 is determined, then the platform frame 646 positionrelative to the carriage guide frame 644 is determined, then the fulcrumframe 630 relative to the platform frame 646 is determined, and then theslave orientation and position relative to the fulcrum frame 630 isdetermined, thereby determining the slave position and orientationrelative to the cart frame 624. It will be appreciated that the slaveposition and orientation relative to the cart frame 624 is determined inthis manner for each arm 10 and in similar fashion for the camera frame610, through its arm 302, relative to the cart frame 624.

Referring to FIG. 6, the position and orientation of the master controlis determined by determining the position of a base frame 648 relativeto the surgeon's station frame 634, then determining the position of theplatform frame 640 relative to the base frame 648, and then determiningmaster position and orientation relative to the platform frame 640. Theposition and orientation of the master frame 622 relative to thesurgeon's station frame 634 is then readily determined through routinecalculation using trigonometric relationships. It will be appreciatedthat the position and orientation of the other master frame relative tothe surgeon workstation 200 frame 634 is determined in a similarfashion.

Referring to FIG. 5, by choosing the frames as described, the setupjoint 95 can be replaced with another setup joint while the same roboticarm is used. The control system can then be programmed with information,e.g., arm lengths and/or the like, relating to the new setup joint only.Similarly, the slave arm 10 can be replaced with another arm, thecontrol system then requiring programming with information, e.g.,fulcrum position and/or the like, relating to the new slave arm only. Itwill be appreciated that in this way the endoscope slave arm 302 and itsassociated setup joint can also be independently replaced, the controlsystem then requiring programming of information relating only to thepart being replaced. Furthermore, referring to FIG. 6, the setup jointand master control can also independently be replaced, the controlsystem requiring programming of information relating to thecharacteristics of the new part only.

FIG. 10 schematically illustrates a high level control architecture fora master/slave robotic system 1000. Beginning at the operator input, asurgeon 1002 moves an input device of a master manipulator 1004 byapplying manual or human forces f_(h) against the input device. Encodersof master manipulator 1004 generate master encoder signals e_(m) whichare interpreted by a master input/output processor 1006 to determine themaster joint positions θ_(m). The master joint positions are used togenerate Cartesian positions of the input device of the master x_(m)relative to the eye frame using a master kinematics model 1008.

Starting now with the input from the surgical environment 1018, thetissue structures in the surgical workspace will impose forces f_(e)against a surgical end effector (and possibly against other elements ofthe tool and/or manipulator). Environmental forces f_(e) from thesurgical environment 1018 alter position of the slave manipulator 1016,thereby altering slave encoder values e_(s) transmitted to the slaveinput/output processor 1014. Slave input/output processor 1014interprets the slave encoder values to determine joint positions θ_(s),which are then used to generate Cartesian slave position signals x_(s)relative to the camera frame according to the slave kinematicsprocessing block 1012.

The master and slave Cartesian positions x_(m), x_(s) are input intobilateral controller 1010, which uses these inputs to generate thedesired Cartesian forces to be applied by the slave f_(s) so that thesurgeon can manipulate the slave as desired to perform a surgicalprocedure. Additionally, bilateral controller 1010 uses the Cartesianmaster and slave positions x_(m), x_(s) to generate the desiredCartesian forces to be applied by the master f_(m) so as to provideforce feedback to the surgeon.

In general, bilateral controller 1010 will generate the slave and masterforces f_(s), f_(m) by mapping the Cartesian position of the master inthe master controller workspace with the Cartesian position of the endeffector in the surgical workspace according to a master-tool transformwhich takes into account a desired scale factor and offsets. The scalefactor specifies a desired scale change between master control inputmovement and responsive slave output movement. The offsets are takeninto account so that the position and orientation of the end effectorframe 618 relative to the camera frame 610 are aligned with the positionand orientation of the master frame 622 relative to the eye frame 612.

The control system 1000 will derive the master-tool transform inresponse to state variable signals provided from the imaging system sothat an image of the end effector in a display appears substantiallyconnected to the input device. These state variables will generallyindicate the Cartesian position of the field of view from the imagecapture device, as supplied by the slave manipulators supporting theimage capture device. Hence, coupling of the image capture manipulatorand slave end effector manipulator is beneficial for deriving thistransform. Clearly, bilateral controller 1010 may be used to controlmore than one slave arm, and/or may be provided with additional inputs.

Based generally on the difference in position between the master and theslave in the mapped workspace, bilateral controller 1010 generatesCartesian slave force f_(s) to urge the slave to follow the position ofthe master. The slave kinematics 1012 are used to interpret theCartesian slave forces f_(s) to generate joint torques of the slaveτ_(s) which will result in the desired forces at the end effector. Slaveinput/output processor 1014 uses these joint torques to calculate slavemotor currents i_(s), which reposition the slave x_(e) within thesurgical worksite.

The desired feedback forces from bilateral controller are similarlyinterpreted from Cartesian force on the master f_(m) based on the masterkinematics 1008 to generate master joint torques τ_(s). The master jointtorques are interpreted by the master input/output controller 1006 toprovide master motor current i_(m) to the master manipulator 1004, whichchanges the position of the hand held input device x_(h) in thesurgeon's hand.

It will be recognized that the control system 1000 illustrated in FIG.10 is a simplification. For example, the surgeon does not only applyforces against the master control device, but also moves the handlewithin the master workspace. Similarly, the motor current supplied tothe motors of the master manipulator may not result in movement if thesurgeon maintains the position of the master controller. Nonetheless,the motor currents do result in tactile force feedback to the surgeonbased on the forces applied to the slave by the surgical environment.Additionally, while Cartesian coordinate mapping is used, the use ofspherical, cylindrical, or other frames may provide at least some of theadvantages of the invention.

In the foregoing, it is assumed that all relevant dimensions and anglesin the system are known for determining the master-tool transform sothat it may be used in controlling movement of the end effector 58 ofthe tool 14 in response to movement of its associated master controldevice 700. However, in some circumstances one or more static lengths orangles of the kinematics structure may not be known or readilydeterminable.

As one example where one or more such lengths and/or angles are notknown, FIG. 11 illustrates a schematic side view of tools 406 a, 406 binserted along with an endoscope 112 through a common entry port 428into a patient. The tools 406 a, 406 b are inserted through and extendout of opposingly curved cannulas (or more generally, tool guides) 416a, 416 b so that their end effectors 408 a, 408 b are positioned toperform a procedure at a work site 424 as shown. The endoscope 112, onthe other hand, is inserted through and extends out of a straightcannula 252 so that the end effectors 408 a, 408 b and work site 424 arein its field of view 430. Each of the cannulas 416 a, 416 b, 252 isinserted into and supported by a port feature 432 at the entry port 428.

The tools 406 a, 406 b and endoscope 112 are coupled to their respectivemanipulators 204 a, 204 b and 242 (only partially shown) that actuatetheir movements relative to their respective fulcrums (also referred toin this case as remote centers of motion) 426, each of which ispositioned to be approximately at the entry port 428 as shown. Each ofthe tools 406 a, 406 b has a flexible shaft so that it may bend toaccommodate its rigid single piece curved cannula. To perform differentprocedures and/or to accommodate different tools, different cannulashaving different curvatures and lengths may be used. Additional detailsof such curved cannulas and systems that use them may be found, forexample, in U.S. application Ser. No. 12/618,583 (filed Nov. 13, 2009;entitled “Curved Cannula Surgical System”), which is incorporated hereinby this reference. It is to be appreciated, however, that cannulashaving difference curvatures and lengths result in different positionsand orientations of the tool's end effector frame as the end effectorextends out of the distal end of such curved cannula.

Although an operator may manually provide to the system anidentification of the curved cannula being used at the time so that thesystem may use known dimensional information for the cannula todetermine the position and orientation of the tool's end effector frame,sometimes the operator may not be aware of which cannula is being used.Thus, in such a case, the unknown position and orientation of the endeffector frame must be determined by the system in some manner in orderto properly control movement of the tool in response to movement of itsassociated master control device.

As another example where one or more such lengths and/or angles are notknown, FIG. 12 illustrates a schematic side view of a tethered camera1220 which captures images of an end effector of a tool 1230 while theend effector is being used to perform a procedure at a work site. A cord1202 is attached at one end 1204 to the camera 1220 and the other end1203 to the patient structure 1201 by conventional means. The positionand/or orientation of the camera tip may be changed by a mechanicalelement 1206 pulling on the camera's tether 1205. Movement of themechanical element 1206 is actuated by a slave manipulator. In thiscase, although the position and orientation of the frame 1210 of themechanical element 1206 may be determined by sensors along its kinematicchain, the lengths of the tether 1205 and cord 1202 and the change inposition and orientation of the camera 1220 resulting from pulling onthe tether 1205 may not be known by the surgeon or system. Thus, in sucha case, the unknown position and orientation of the camera frame 1221must be determined by the system in some manner in order to properlycontrol movement of the tool relative to the camera frame 1221 inresponse to movement of its associated master control device. Signalsfrom the camera 1220 may be transmitted through fiber optic cablesdisposed within or alongside the tether 1205 or alternatively, they maybe transmitted wirelessly.

As yet another example where one or more such lengths and/or angles arenot known, FIG. 13 illustrates a schematic side view of movablestructures 1312, 1302 for respectively positioning a roboticallycontrolled tool 1314 and image capturing system 1304 at a work site. Inthis case, the system is able to determine the position and orientationof the image reference frame of the image capturing system 1304 relativeto a frame 1301 associated with the movable structure 1302 by usingjoint position sensors and known dimensions of the robotic arm 1303. Itis also able to determine the position and orientation of the tool framerelative to a frame 1311 associated with the movable structure 1312 byusing joint position sensors and known dimensions of the robotic arm1313. However, the distance and/or relative orientation between themovable structures 1312, 1302 may not be known. Thus, in such a case,the unknown distance and/or relative orientation between the movablestructures 1312, 1302 or alternatively between their respective frames1311, 1301 must be determined by the system in some manner in order toproperly control movement of the tool 1314 relative to the camera framein response to movement of tool 1314's associated master control device.

In the present invention, rather than estimating the unknown lengthsand/or angles for determining the master-tool transform, an unknownposition and orientation of a frame corresponding to the unknown lengthand/or angle is estimated instead by estimating a transform to theunknown position and orientation from known positions and orientationsof other frames. For examples, in reference to FIG. 11, the unknownposition and orientation of the end effector frame is determined by theprocessor 102 estimating a transform _(e) ^(n)T which transforms pointsin an end effector frame to corresponding points in a slave manipulatorframe; in reference to FIG. 12, the unknown position and orientation ofthe camera frame is determined by the processor 102 estimating atransform _(c) ^(k)T which transforms points in the image frame 1221 tocorresponding points in a mechanical element frame 1210; and inreference to FIG. 13, the unknown distance and/or relative orientationbetween the movable structures 1302, 1312 is determined by the processor102 estimating a transform ₁₃₀₁ ¹³¹¹T which transforms points in themovable structure frame 1301 to corresponding points in the movablestructure frame 1311 which is assumed in this case to be a fixed frame.For details on the computation, manipulation, and nomenclature of suchtransforms, see, e.g., John J. Craig, Introduction to Robotics Mechanicsand Control, 2^(nd) Edition, Addison-Wesley Publishing Company, Inc.,1989.

A framework for describing the invention is illustrated in FIG. 14 whichshows a generalized schematic diagram of image frame transforms (e.g.,_(k) ^((k-1))T, _(c) ^(k)T) and tool transforms (e.g., _(n) ^((n-1))T,_(e) ^(n)T). The image frame transforms in this case are used fortransforming or mapping points between an image frame 610, intermediateframes (e.g., 1412, 1413), and a fixed frame 1401. Likewise, the toolframe transforms are used for transforming or mapping points between atool frame 618, intermediate frames (e.g., 1402, 1403), and the fixedframe 1401.

FIG. 15 illustrates, as an example, a flow diagram of a method forestimating and periodically updating a master-tool frame transform forcontrolling movement of a tool in response to movement of an associatedmaster control device in a robotic system. In 1501, a processorimplementing the method determines setup transforms such as thoserelated to the setup arm 95 for the cart 300, as described in referenceto FIG. 5, which is locked in place after the tool 14 is properlypositioned in the entry port to perform a procedure at a work site.Since these transforms do not change in this example during theprocedure being performed by the tool, it is not necessary for them tobe continually updated as part of the processing loop.

In 1502, the processor receives access to information of an image thathas been captured by an image capturing system such as the endoscope304. As an example, the image information may be stored in a framebuffer from which it is to be displayed on the display 204. In 1503, adetermination is made whether a vision measurement is to be performed.It is to be appreciated that although the frame buffer is updatedperiodically to refresh images being displayed on the display 204, avision measurement need not be performed each time the frame buffer isupdated. Therefore, a vision measurement might be performed periodicallyafter a number of frame buffer refresh times.

If the determination in 1503 is NO, then the processor loops back to1502. On the other hand, if the determination is YES, then the processorproceeds to 1504 where the processor determines the slave manipulatortransforms such as those related to the slave arm 10 coupled to the cart300 as described in reference to FIG. 5, by using sensed joint and/orlinkage positions and known dimensions of the slave arm 10. Although1504 is shown as being performed each time a vision measurement isperformed, it is to be appreciated that a slave manipulator transformneed only be determined if the corresponding slave manipulator has beenmoved (translationally or orientationally) since the last time its slavemanipulator transform has been determined. For example, if the positionand orientation of the image capturing device 304 is held fixed whilethe tool 14 is being used to perform a procedure at a work site, then itis not necessary to determine its slave manipulator transform(s) morethan once during the period it is being held in the fixed position andorientation.

In 1505, the processor identifies pose indicating points of the endeffector and/or other kinematic structure in the captured image. To dothis, measurements of features related to the position and orientation(i.e., pose) of the end effector and/or other kinematic structure aremade from the images. The measurements can be in different formsdepending on the type of camera being used to capture the image (e.g.,monocular or stereo) and the types of image features or fiducial markersbeing identified in the captured image.

If a stereo camera is being used, the depth of a point can be determinedby the process of stereo triangulation, given the intrinsic parametersof the two cameras and their relative pose. Distinctive features areneeded to separate themselves from other image features generated byanother part of the tool, robot arm, and background. Fiducial markersare used in order to create distinctive features. The fiducial markerscan be distinctive in spectral property (e.g., color), in shape, ingeometric configuration, or in their combinations. Examples of suchmarkers and their use are described in U.S. Patent Application Pub. No.US 2010/0168562 A1 (filed Apr. 23, 2009; entitled “Fiducial MarkerDesign and Detection for Locating Surgical Instrument in Images”) andU.S. Patent Application Pub. No. US 2010/0168763 A1 (filed Apr. 23,2009; entitled “Configuration Marker Design and Detection for InstrumentTracking”), both of which are incorporated herein by reference.

If a monocular camera is used, the measurements are inherently in thetwo-dimensional (2D) image domain. However, having certain knowledge ofthe tool and/or fiducial marker(s) on the tool can still makethree-dimensional (3D) measurements possible. Given the intrinsicparameters of the camera, the 3D pose of an object of known geometry canbe recovered with 2D-to-3D point correspondences by a process calledpose estimation, see, e.g., David A. Forsyth and Jean Ponce, ComputerVision: A Modern Approach, Prentice Hall, 2003. Pose or partial pose ofspecial geometric shapes (e.g., circles, spheres, cylinders, and cones)can be computed from their outlines, see, e.g., Yiu C. Shiu and HanqiZhuang, “Pose Determination of Cylinders, Cones, and Spheres fromPerspective Projections,” Applications of Artificial intelligence X:Machine Vision and Robotics, Proceedings SPIE, Vol. 1708, pp. 771-782,March 1992. Such geometric shapes are common in surgical instruments ortools.

If the 2D measurements in the image domain cannot be directly convertedinto 3D using the above techniques, then the 3D pose may be estimatedinstead. In this case, the image projection u_(i)=[u_(i), v_(i)]^(t) ofa 3D point _(c)p_(i)=[_(c)X_(i), _(c)y_(i), _(c)z_(i)]^(T) may becomputed by:

$\begin{matrix}\left\{ \begin{matrix}{u_{i} = {{f_{x}\frac{cxi}{czi}} + u_{0}}} \\{v_{i} = {{f_{y}\frac{cyi}{czi}} + v_{0}}}\end{matrix} \right. & (1)\end{matrix}$

wherein f_(x) and f_(y) are the focal length in horizontal and verticaldirections of an imager respectively, and u₀ and v_(o) are the principalpoint.

In 1506, the processor estimates an unknown transform using thetransforms and other information determined in 1504 and 1505. In doingso, it uses the following transform equation:

_(c) ^(e) T= _(w) ^(e) T· _(c) ^(w) T  (2)

where _(c) ^(e)T is the image (or camera “c”) frame to tool (or endeffector “e”) frame transform (which may be determined mathematicallyafter identifying pose indicating points of the end effector in theimage frame in 1505), _(c) ^(w)T is the image frame to fixed (or world“w”) frame transform, and _(w) ^(e)T is the inverse of tool frame tofixed frame transform.

Both transforms _(c) ^(w)T and _(w) ^(e)T may involve a chain oftransforms. As an example, referring to FIG. 14, if frame 1401 is thefixed frame (i.e., a stationary frame), then the chain of transformsfrom the tool frame to the fixed frame (referred to herein as the “toolframe transforms”) includes the frame 618 to frame 1403 transform at itsstart and the frame 1402 to frame 1401 transform at its end. Also, thechain of transforms from the image frame to the fixed frame (referred toherein as the “image frame transforms”) includes the frame 610 to frame1413 transform at its start and the frame 1412 to frame 1401 at its end.The inverse of the tool frame transforms would then start with the frame1401 to frame 1402 transform and end with the frame 1403 to frame 618transform.

Thus, if the unknown transform is the frame 1403 to frame 618 transform(^(e) _(n)T), it can be determined using transform equation (2) asfollows:

_(c) ^(e) T= _(w) ^(e) T· _(c) ^(w) T= _(n) ^(e) T· _(w) ^(n) T· _(c)^(w) T  (3)

_(n) ^(e) T= _(c) ^(e) T· _(w) ^(c) T· _(n) ^(w) T  (4)

Such a solution can be used to estimate the position and orientation ofthe end effector as required in reference to FIG. 11 for cannulas ofdifferent curvatures and lengths.

On the other hand, if the unknown transform is the frame 1413 to frame610 transform ('T), it can be determined using transform equation (2) asfollows:

_(c) ^(e) T= _(w) ^(e) T· _(c) ^(w) T= _(w) ^(e) T· _(k) ^(w) T· _(c)^(k) T  (5)

_(k) ^(c) T= _(e) ^(c) T· _(w) ^(e) T· _(k) ^(w) T  (6)

Such a solution can be used to estimate the position and orientation ofthe camera as required in reference to FIG. 12 for the tethered camera.

Still further, if the unknown transform is the frame 1402 to the fixedframe transform (^(w) ₁₄₀₂T) it can be determined using equation (2) asfollows:

_(c) ^(e) T= _(w) ^(e) T· _(c) ^(w) T= ₁₄₀₂ ^(e) T· _(w) ¹⁴⁰² T· _(c)^(w) T  (7)

₁₄₀₂ ^(w) T= _(c) ^(w) T· _(e) ^(c) T· ₁₄₀₂ ^(e) T  (8)

Such a solution can be used to estimate the position and/or relativeorientation of one movable structure relative to another movablestructure as required in reference to FIG. 13 for the two movablestructures 1302, 1311 where the frame of one of the movable structuresis chosen as the fixed frame. Although both structures 1302, 1311 areassumed movable, the solution is also applicable where one of thestructures 1302, 1311 is fixed in position (such as with a ceilingmounted robotic arm) to define the fixed reference frame while the otherone of the structures 1302, 1311 is movable (such as on a patient sidecart).

The above examples assume that c^(e)T is first determined by theprocessor so that it may perform computations in equations (3) through(8). As an alternative approach, a point in the tool frame ^(e)p may beassociated with a point in the image ^(c)p by the followingtransformation:

$\begin{matrix}{{\,_{c}^{e}T} = \begin{bmatrix}R & T \\{0,0,0} & 1\end{bmatrix}} & (9)\end{matrix}$

where R is a 3×3 rotation matrix and T is a 3D translation vector, sothat:

^(e) p= _(c) ^(e) T· ^(c) p  (10)

Thus, by substituting the right side of equation (2) for _(c) ^(e)T inequation (10), points in the image frame may be directly mapped intopoints in the tool frame without first determining _(c) ^(e)T. As anexample of applying this alternative approach to performing block 1506,equation (10) may be rewritten in general form as follows:

^(e) p= _(n) ^(e) T· _((n-1)) ^(n) T· _(w) ^((n-1)) T· _((k-1)) ^(w) T·_(k) ^((k-1)) T· _(c) ^(k) T· ^(c) p  (11)

Now, as an example, if the transform _(k) ^((k-1))T is the unknowntransform and its values are not time varying, then equation (11) may berewritten as follows:

$\begin{matrix}{{{\,_{({k - 1})}^{k}T} \cdot \underset{\underset{{({k - 1})}_{p}}{}}{{\,_{w}^{({k - 1})}T} \cdot {\,_{({n - 1})}^{w}T} \cdot {\,_{n}^{({n - 1})}T} \cdot {\,_{e}^{n}T} \cdot {\,^{e}p}}} = \underset{\underset{\,^{k}p}{}}{{\,_{c}^{k}T} \cdot {\,^{c}p}}} & (12)\end{matrix}$

In this form, the unknown transform _((k-1)) ^(k)T (and consequently,its inverse transform _(k) ^((k-1))T) can be solved by a set of pointpairs {^((k-1))p_(i), ^(k)p_(i)} where i=1 . . . m. The point pairs canbe derived from a single time instant or from a number of time instants.As examples of methods for solving equation (12), see, e.g., A. Lorusso,D. W. Eggert, and R. B. Fisher “A Comparison of Four Algorithms forEstimating 3-D Rigid Transformations,” Proceedings BMVC, 1995.

If some knowledge of the transform _((k-1)) ^(k)T is known (e.g., thenumber of independent parameters in the transform is fewer than 6), thetransform can be rewritten as:

^(k) p=f(Θ;^((k-1)) p)  (13)

where Θ={θ₁, . . . θ_(a)} are the free parameters. In this case, Θ inthe most general form, can be solved by minimizing the following costfunction using a standard non-linear optimization routine (e.g.,Gauss-Newton method):

Θ*=arg min_(Θ)Σ_(i=1) ^(m)∥^(k) p _(i) −f(Θ;^((k-1)) p _(i)∥²  (14)

Note that equation (14) applies to 3D points. For 2D points asapplicable to equation (1), the parameters of the unknown transform maybe solved by optimizing the following cost function:

Θ-=arg min_(Θ)Σ_(i=1) ^(m) ∥u _(i) −u′ _(i)∥²  (15)

where u′_(i)==[u′_(i), v′_(i)]^(t) are the image coordinates of a pointand u_(i)=[u_(i), v_(i)]^(t) is the image projection of the 3D point^(c)p_(i)=[^(c)x_(i), ^(c)y_(i), ^(c)z_(i)]^(T).

If the camera parameters are unknown, the parameters can be estimatedtogether with the unknown transform to be solved. If the lens is notperfect and has distortion (e.g., radial, tangential), the distortionparameters of the lens can also be estimated in a similar fashion as thecamera intrinsic parameters (not shown here). For example, equation (15)may be modified as follows:

{Θ,f_(x) ,f _(y) u ₀ ,v ₀}*=arg min_({Θ,f) _(x) _(,f) _(y) _(,u) ₀ _(,v)_(0}) Σ_(i=1) ^(m) ∥u _(i) −u′ _(i)∥²  (16)

As can be appreciated from the above examples; determining one unknowntransform among the image and tool frame transforms is straightforwardwhen all other transforms are known. When unknown parameters from morethan one transform needs to be estimated, the parameters can beestimated by optimization similar to equations (14), (15) and (16).

In addition to solving an unknown transform by a set of pointcorrespondences, filtering can be used for fitting the transform or theoptimization. A filter from the Kalman filter family can be used. Inparticular, extended Kalman filters and unscented Kalman filters can beused for non-linear systems. An iterated extended information filterapplicable for such use is described, for example, in U.S. patentapplication Ser. No. 12/495,304 (filed Jun. 30, 2009; entitled“Efficient Vision and Kinematic Data Fusion for Robotic SurgicalInstruments and Other Applications”), which is incorporated herein bythis reference. Benefits of using a filter include temporal smoothnessand computational efficiency.

Even if a filter is not used, temporal smoothing on the estimatedparameters over time can avoid abrupt change. In 1507, the processoroptionally may perform temporal smoothing of the estimated unknowntransform rather than immediately applying it to update the master-toolframe transform. Such smoothing would avoid abrupt changes in thecontrol of the tool by its associated master control device. One methodof performing such smoothing is to maintain a running average ofestimated values for the unknown transform as the processor continuallyloops through the method by performing 1502-1508.

In 1508, the processor updates the master-tool frame transform using thefiltered version of the estimated unknown transform and command movementof the tool in response to movement of the master control device usingthe updated master-tool transform. For example, a modified tool frame tocamera frame transform using the filtered version of the estimatedunknown transform may be determined. Then the master frame to eye frametransform may be modified so that the position and orientation of thetool frame 618 relative to the camera frame 610 are aligned with theposition and orientation of the master frame 622 relative to the eyeframe 612. After performing 1508, the processor then jumps back to 1502to process the next vision measurement cycle.

Although the various aspects of the present invention have beendescribed with respect to an embodiment, it will be understood that theinvention is entitled to full protection within the full scope of theappended claims.

1. A robotic system comprising: a master control device having a masterframe; an image capturing system having an image frame whose positionand orientation relative to a fixed frame is determinable through one ormore image frame transforms; a tool disposed within a field of view ofthe image capturing system and having a tool frame whose position andorientation relative to the fixed frame is determinable through one ormore tool frame transforms; and at least one processor programmed toidentify pose indicating points of the tool from information in one ormore images captured by the image capturing system, determine anestimated transform for an unknown one of the image and tool frametransforms by using the identified pose indicating points and known onesof the image and tool frame transforms, update a master-to-tooltransform by using the estimated and known ones of the image and toolframe transforms, and command movement of the tool in response tomovement of the master control device by using the updatedmaster-to-tool transform.
 2. The robotic system of claim 1, furthercomprising: a display that is viewable by an operator wherein an eyeframe is defined at the eyes of the operator; and a vision systemconfigured to display information of images captured by the imagecapturing system in the display; wherein the at least one processorupdates the master-to-tool transform by adjusting an offset which alignsan eye-master transform with that of a image-tool transform, wherein theeye-master transform transforms points in the master frame tocorresponding points in the eye frame, and wherein the image-tooltransform transforms points in the tool frame to corresponding points inthe image frame.
 3. The robotic system of claim 1, wherein the unknownone of the image and tool frame transforms is one of the one or moretool frame transforms.
 4. The robotic system of claim 3, wherein thetool moves relative to a pivot point having a pivot point frame, thetool has an end effector which has an end effector frame, and theunknown tool frame transform transforms points in the pivot point frameto corresponding points in the end effector frame.
 5. The robotic systemof claim 4, wherein the unknown tool frame transform identifies a toolguide through which the tool extends so that the end effector extendsout of a distal end of the tool guide, and wherein the identified toolguide is one of a number of candidate tool guides having different pivotpoint to end effector transforms.
 6. The robotic system of claim 1,wherein the unknown one of the image and tool frame transforms is one ofthe one or more image frame transforms.
 7. The robotic system of claim6, wherein the image capturing device is tethered to a mechanicalelement having a mechanical element frame, and wherein the unknown imageframe transform transforms points in the mechanical element frame tocorresponding points in the image frame.
 8. The robotic system of claim1, further comprising: a first structure having a first robotic armcoupled to the image capturing device, the first structure having afirst structure frame; and a second structure having a second roboticarm coupled to the tool, the second structure having a second structureframe; wherein one of the first and second structure frames is the fixedframe and the unknown one of the image and tool frame transforms is atransform that transforms points in the other of the first and secondstructure frames to corresponding points in the fixed frame.
 9. Therobotic system of claim 8, wherein the first or second structurecorresponding to the other of the first and second structure frames is amovable structure.
 10. The robotic system of claim 8, wherein the firstor second structure corresponding to the fixed frame is a movablestructure.
 11. The robotic system of claim 1, wherein one or more of theknown ones of the image frame transforms are determined using one ormore sensors positioned along a kinematic chain extending to the imagecapturing device.
 12. The robotic system of claim 1, wherein one or moreof the known ones of the image frame transforms are determined byapplying trigonometry to known dimensions of one or more structuresmechanically coupled to the image capturing device.
 13. The roboticsystem of claim 1, wherein one or more of the known ones of the toolframe transforms are determined using one or more sensors positionedalong a kinematic chain extending to the tool.
 14. The robotic system ofclaim 1, wherein one or more of the known ones of the tool frametransforms are determined by applying trigonometry to known dimensionsof one or more structures mechanically coupled to the tool.
 15. Therobotic system of claim 1, wherein the at least one processor smoothesthe estimated transform for the unknown one of the image and tool frametransforms and updates the master-to-tool transform using the smoothedestimated transform.
 16. The robotic system of claim 15, wherein the atleast one processor is programmed to estimate the position andorientation of the tool frame relative to the image frame byperiodically updating a prior estimate of the position and orientationof the tool frame relative to the image frame, determine the estimatedtransform for the unknown one of the image and tool frame transforms byupdating a prior determination for the estimated transform for theunknown one of the image and tool frame transforms, and smooth theestimated transform for the unknown one of the image and tool frametransforms by averaging current and prior estimated transforms for theunknown one of the image and tool frame transforms.
 17. A methodimplemented in a robotic system comprising a master control devicehaving a master frame, an image capturing system having an image framewhose position and orientation relative to a fixed frame is determinablethrough one or more image frame transforms, a tool disposed within afield of view of the image capturing system and having a tool framewhose position and orientation relative to the fixed frame isdeterminable through one or more tool frame transforms, the methodcomprising: identifying pose indicating points of the tool frominformation in one or more images captured by the image capturingsystem; determining an estimated transform for an unknown one of theimage and tool frame transforms by using the identified pose indicatingpoints and known ones of the image and tool frame transforms; updating amaster-to-tool transform by using the estimated and known ones of theimage and tool frame transforms; and commanding movement of the tool inresponse to movement of the master control device by using the updatedmaster-to-tool transform.
 18. The method of claim 17, wherein therobotic system further comprises a display that is viewable by anoperator wherein an eye frame is defined at the eyes of the operator anda vision system configured to display information of images captured bythe image capturing system in the display, and wherein the updating ofthe master-to-tool transform comprises: adjusting an offset which alignsan eye-master transform with that of an image-tool transform, whereinthe eye-master transform transforms points in the master frame tocorresponding points in the eye frame, and wherein the image-tooltransform transforms points in the tool frame to corresponding points inthe image frame.
 19. The method of claim 17, wherein the unknown one ofthe image and tool frame transforms is one of the one or more tool frametransforms.
 20. The method of claim 19, wherein the tool moves relativeto a pivot point having a pivot point frame, the tool has an endeffector which has an end effector frame, and the unknown tool frametransform transforms points in the pivot point frame to correspondingpoints in the end effector frame.
 21. The method of claim 20, whereinthe unknown tool frame transform identifies a tool guide through whichthe tool extends so that the end effector extends out of a distal end ofthe tool guide, and wherein the identified tool guide is one of a numberof candidate tool guides having different pivot point to end effectortransforms.
 22. The method of claim 17, wherein the unknown one of theimage and tool frame transforms is one of the one or more image frametransforms.
 23. The method of claim 22, wherein the image capturingdevice is tethered to a mechanical element having a mechanical elementframe, and wherein the unknown image frame transform transforms pointsin the mechanical element frame to corresponding points in the imageframe.
 24. The method of claim 17, wherein the robotic system furthercomprises a first structure having a first robotic arm coupled to theimage capturing device, the first structure having a first structureframe, and a second structure having a second robotic arm coupled to thetool, the second structure having a second structure frame; and whereinone of the first and second structure frames is the fixed frame and theunknown one of the image and tool frame transforms is a transform thattransforms points in the other of the first and second structure framesto corresponding points in the fixed frame.
 25. The method of claim 24,wherein the first or second structure corresponding to the other of thefirst and second structure frames is a movable structure.
 26. The methodof claim 24, wherein the first or second structure corresponding to thefixed frame is a movable structure.
 27. The method of claim 17, furthercomprising: determining one or more of the known ones of the image frametransforms by using one or more sensors positioned along a kinematicchain extending to the image capturing device.
 28. The method of claim17, further comprising: determining one or more of the known ones of theimage frame transforms by applying trigonometry to known dimensions ofone or more structures mechanically coupled to the image capturingdevice.
 29. The method of claim 17, further comprising: determining oneor more of the known ones of the tool frame transforms by using one ormore sensors positioned along a kinematic chain extending to the tool.30. The method of claim 17, further comprising: determining one or moreof the known ones of the tool frame transforms by applying trigonometryto known dimensions of one or more structures mechanically coupled tothe tool.
 31. The method of claim 17, further comprising: smoothing theestimated transform for the unknown one of the image and tool frametransforms; and wherein the updating of the master-to-tool transformuses the smoothed estimated transform.
 32. The method of claim 31,wherein the estimating of the position and orientation of the tool framerelative to the image frame comprises periodically updating a priorestimate of the position and orientation of the tool frame relative tothe image frame; the determining of the estimated transform for theunknown one of the image and tool frame transforms comprises updating aprior determination for the estimated transform for the unknown one ofthe image and tool frame transforms; and the smoothing of the estimatedtransform for the unknown one of the image and tool frame transformscomprises averaging current and prior estimated transforms for theunknown one of the image and tool frame transforms.